#!/usr/bin/env roseus
;; -*- mode: EusLisp; coding: utf-8-unix; -*- 
#-:jsk
(jsk)
#-:rbrain-basic
(rbrain)

(ros::roseus "arduino_sample")

(defconstant +x_thr+ 680)
(defconstant +z_thr+ 600)
(defconstant +x_gain+ 0.3)
(defconstant +z_gain+ 0.5)

(defun acc-cb (msg)
  (let ((pub-cmd-vel (instance geometry_msgs::Twist :init)))
    (ros::ros-info "accerelation subscribed")
    (if (or (and (< +x_thr+ (send msg :x))
                 (< (send msg :z) (send msg :x)))
            (and (< (send msg :x) +x_thr+)
                 (< (send msg :x) (send msg :z))))
        (if (+x_thr+ < (send msg :x))
            (send pub-cmd-vel :linear :x
                  (- (send msg :x) +x_thr+))
          (send pub-cmd-vel :linear :x
                (- +x_thr+ (send msg :x))))
      (if (+z_thr+ < (send msg :z))
            (send pub-cmd-vel :linear :z
                  (- (send msg :z) +z_thr+))
          (send pub-cmd-vel :linear :z
                (- +z_thr+ (send msg :z)))))
    (ros::publish "cmd_vel" pub-cmd-vel)))

(ros::subscribe "/vector" geometry_msgs::Vector3 #'acc-cb)
(ros::advertise "cmd_vel" geometry_msgs::Twist 1)

(defun ros-proc ()
  (do-until-key
   (ros::spin-once))
  )
(ros-proc)